#define _GNU_SOURCE
#include <sys/stat.h>
#include <sys/types.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>
#include <stdio.h>
#include <errno.h>
#include <time.h>
#include <dlfcn.h>
#include <pthread.h>
#include <assert.h>
#include "vm.h"
#include "util.h"

#define CASE(n, f)  case n: f(core); break;
#define PC_AT ((((int16_t)(*core).reg.pc-(int16_t)program)/INSTR_BYTES)-1)

#define opcode instr->opcode
#define reg1   instr->reg1
#define reg2   instr->reg2
#define reg3   instr->reg3
#define val    instr->val

pthread_mutex_t ivt_mtx = PTHREAD_MUTEX_INITIALIZER;
bool (*ivt[MAX_INTERRUPTS])();    //Interrupt Vector Table
int16_t memory[MAX_MEM];          //Memory
int16_t hwmemory[MAX_HWMEM];      //Hardware-reserved memory
int hwp=0;                        //Pointer to remaining unallocated hardware space
instruction* instr;               //Current instruction
char* program;                    //Buffer containing whole binary program
char* save[MAX_JMP];              //JMPS stack 
int savep=0;                      //Position in JMPS stack
int state=1;                      //VM running state
hardwareObject hardware[MAX_DEV]; //Hardware table
int hardware_cnt=0;               //Current number of attached hardware devices

//Setup our mutex locks
pthread_mutex_t decode_lock;
pthread_mutex_t memory_lock[MAX_HWMEM];
pthread_mutex_t reg_lock[REGS];
pthread_mutex_t reg_save_lock;

void vsleep() { while (1) { getc(stdin); } }

void split(char *_s, char* arr[]) {
  int l;
  int i=0;
  char* s = strndup(_s, MAX_LINE);
  char* token;

  while (token = strsep(&s, " ")) {
    arr[i] = strndup(token, strlen(token));
    i++;
  }

  free(s);
}

void decode(core_t *core) {
  pthread_mutex_lock(&decode_lock);
  memcpy(instr, (instruction*)(*core).reg.pc, INSTR_BYTES);
  (*core).reg.pc += INSTR_BYTES;
  pthread_mutex_unlock(&decode_lock);
}

void dumpRegs(core_t *core) {
  printf("REGISTER DUMP\n");
  for (int i=0; i<REGS; i++) {
    printf("Register %d: %4d [0x%x]\n", i, (*core).reg.regs[i], (*core).reg.regs[i]);
  }
}

void dumpMemCell(int i) {
  int16_t p=0;
  p = memget(i, 16);
  printf("[@%x:%2x] ", i,
         p);
}

void dumpMem() {
  int i; int i2; 
  printf("SYSTEM MEMORY DUMP\n");
  while (i < MAX_MEM) {
    dumpMemCell(i); i++;
    dumpMemCell(i); i++;
    dumpMemCell(i); i++;
    dumpMemCell(i); i++;
    putc('\n', stdout);    
  }
}

void dumpHwMem() {
  int i=HWMEM_START; int i2; 
  printf("HARDWARE MEMORY DUMP\n");
  while (i < (HWMEM_START+MAX_HWMEM)) {
    dumpMemCell(i); i++;
    dumpMemCell(i); i++;
    dumpMemCell(i); i++;
    dumpMemCell(i); i++;
    putc('\n', stdout);    
  }
}

void dumpInstr(core_t *core) {
  int i;
  logs("Decoded instruction @%2x: |%2x|%3x|%3x|%3x|%5x", PC_AT, opcode, reg1, reg2, reg3, val);
}

void dumpProg(size_t sz) {
  int i;
  printf("PROGRAM DUMP\n");
  for (i=0; i<sz; i++) { printf("%x ", program[i]); }
  putc('\n', stdout);
}

void hwerr(char* reason) {
  printf("FATAL:  %s", reason);
  _halt();
}

void memerr(int16_t addr) {
  printf("FATAL: Invalid address '%x'", addr);
  _halt();
}

void reg_save(core_t *core) {
  uint16_t set_banks;
  uint_fast8_t use_bank;
  /* First, let's see what we have for saved registers */
  set_banks = (*core).reg.cst & 0xF00;
  if (set_banks >= 7) {
    hwerr("Register bank saturation");
  }
  switch(set_banks) {
  case 0:   // No banks in use
    use_bank = 0;
    set_banks = 1;
    break;
  case 1:   //first bank in use
    use_bank = 1;
    set_banks = 3;
    break;
  case 2:   //second bank in use, but not first
    use_bank = 0;
    set_banks = 3;
    break;
  case 3:   //first and second banks in use
    use_bank = 2;
    set_banks = 7;
    break;
  case 4:   //just third bank in use
    use_bank = 0;
    set_banks = 5;
    break;
  case 5:   //first and third banks in use
    use_bank = 1;
    set_banks = 7;
    break;
  case 6:   //second and third banks in use
    use_bank = 0;
    set_banks = 7;
    break;
  }
  pthread_mutex_lock(&reg_save_lock);
  (*core).reg.cst = (set_banks << 8) + (use_bank << 4) + (2);
  (*core).breg[use_bank] = (*core).reg;
  pthread_mutex_unlock(&reg_save_lock);
}

void reg_load(core_t *core, uint16_t bank) {
  /* if bank is set to -1, then get most recently saved bank*/
  uint16_t status;
  uint16_t set_banks;
  status = (*core).reg.cst;
  if (bank == -1) {
    bank = (status & 0xF0) >> 4;
  }
  if (bank > BREGS) {
    hwerr("Attempted to access nonexistent bank");
  }
  set_banks = (status & 0xF00) >> 8;
  switch(bank) {
  case 0:
    set_banks -= 1;
    break;
  case 1:
    set_banks -= 2;
    break;
  case 2:
    set_banks -= 4;
    break;
  }
  pthread_mutex_lock(&reg_save_lock);
  (*core).reg = (*core).breg[bank];
  (*core).reg.cst = ((*core).reg.cst & 0xF0FF) + (set_banks << 8);
  pthread_mutex_unlock(&reg_save_lock);
}

int16_t memget(int16_t addr, uint_fast8_t bits) {
  int16_t mem;
  if (addr < 0) { memerr(addr); }
  if (addr > MAX_MEM) {
    addr -= MAX_MEM;
    if (addr > MAX_HWMEM) { memerr(addr); }      
    pthread_mutex_lock(&memory_lock[addr]);
    mem = hwmemory[addr];
    pthread_mutex_unlock(&memory_lock[addr]);
  }
  else {
    pthread_mutex_lock(&memory_lock[addr]);
    mem = memory[addr];
    pthread_mutex_unlock(&memory_lock[addr]);
  }
  return (mem & bitmask(bits));
}


bool memstore(int16_t addr, int16_t value, uint_fast8_t bits) {
  if (addr < 0) { memerr(addr); }
  uint16_t mask = bitmask(bits);
  uint16_t rmask = 0xffff ^ mask;
  if (addr > MAX_MEM) {
    addr -= MAX_MEM;
    if (addr > MAX_HWMEM) { memerr(addr); }
    pthread_mutex_lock(&memory_lock[addr]);
    value = (mask & value) | (rmask & hwmemory[addr]);
    hwmemory[addr] = value;
    pthread_mutex_unlock(&memory_lock[addr]);
    return TRUE;
  }
  else {
    pthread_mutex_lock(&memory_lock[addr]);
    value = (mask & value) | (rmask & memory[addr]);
    memory[addr] = value;
    pthread_mutex_unlock(&memory_lock[addr]);
    return TRUE;
  }
  return FALSE;
}

void cmp(int a, int b, core_t *core) {
  (*core).reg.regs[8] = 0;
  if      (a > b)  { SETBIT((*core).reg.regs[8], GT); }
  else if (a < b)  { SETBIT((*core).reg.regs[8], LT); }
  else if (a == b) { SETBIT((*core).reg.regs[8], EQ); }
}

void _add(core_t *core)  { (*core).reg.regs[reg1] =
                           (*core).reg.regs[reg2] + (*core).reg.regs[reg3]; }
void _sub(core_t *core)  { (*core).reg.regs[reg1] =
                           (*core).reg.regs[reg2] - (*core).reg.regs[reg3]; }
void _mlt(core_t *core)  { (*core).reg.regs[reg1] =
                           (*core).reg.regs[reg2] * (*core).reg.regs[reg3]; }
void _div(core_t *core)  {
  if ((*core).reg.regs[reg2] && (*core).reg.regs[reg3]) {
    (*core).reg.regs[reg1] = (*core).reg.regs[reg2] / (*core).reg.regs[reg3];
    return;
  }
  (*core).reg.regs[reg1] = 0;
}
void _mod(core_t *core)  { (*core).reg.regs[reg1] =
                           (*core).reg.regs[reg2] % (*core).reg.regs[reg3]; }
void _ldi(core_t *core)  { (*core).reg.regs[reg1] = val;}
void _ldr(core_t *core)  { (*core).reg.regs[reg1] = (*core).reg.regs[reg2];    }
void _ldm(core_t *core)  { (*core).reg.regs[reg1] = memget(val, 8); }
void _st16(core_t *core) { memstore(val, (*core).reg.regs[reg1], 16);}
void _st8(core_t *core)  { memstore(val, (*core).reg.regs[reg1], 8);}
void _and(core_t *core)  { (*core).reg.regs[reg1] =
                           (*core).reg.regs[reg2] & (*core).reg.regs[reg3]; }
void _or(core_t *core)   { (*core).reg.regs[reg1] =
                           (*core).reg.regs[reg2] | (*core).reg.regs[reg3]; }
void _xor(core_t *core)  { (*core).reg.regs[reg1] =
                           (*core).reg.regs[reg2] ^ (*core).reg.regs[reg3]; }
void _not(core_t *core)  { (*core).reg.regs[reg1] = ~(*core).reg.regs[reg1];             }
void _jmp(core_t *core)  {
  (*core).reg.pc = (program+(INSTR_BYTES*val));
  logs("JMP to: %x", val);
}
void _jmps(core_t *core) {
  save[savep] = (*core).reg.pc;
  savep++;
  _jmp(core);
}
void _rst(core_t *core)  {
  savep--;
  (*core).reg.pc = save[savep];
  logs("RST to: %x", PC_AT);
}
void _nop(core_t *core)  { return; }
void _dbg(core_t *core)  {
  switch (val) {
  case 0: dumpRegs(core); break;
  case 1: dumpMem();  break;
  case 2: dumpHwMem();  break;
  case 3: vsleep(); break;
  }
}
void _loop(core_t *core) {
  if ((*core).reg.regs[reg1] > 0) {
    (*core).reg.regs[reg1]--;
    _jmp(core);
  }
}
void _int(core_t *core)  { interrupt(val, core); }
void _shl(core_t *core)  { (*core).reg.regs[reg1] =
                           (*core).reg.regs[reg2] << (*core).reg.regs[reg3];  }
void _shr(core_t *core)  { (*core).reg.regs[reg1] =
                           (*core).reg.regs[reg2] >> (*core).reg.regs[reg3];  }
void _shli(core_t *core) { (*core).reg.regs[reg1] =
                           (*core).reg.regs[reg2] >> val;}
void _shri(core_t *core) { (*core).reg.regs[reg1] =
                           (*core).reg.regs[reg2] >> val;}
void _crr(core_t *core)  { cmp((*core).reg.regs[reg1],(*core).reg.regs[reg2], core);             }
void _cri(core_t *core)  { cmp((*core).reg.regs[reg1],val, core);                    }
void _crm(core_t *core)  { cmp((*core).reg.regs[reg1],memget(val, 16), core);        }
void _jgt(core_t *core)  { if (CHECKBIT((*core).reg.regs[8], GT)) { _jmp(core); }  }
void _jlt(core_t *core)  { if (CHECKBIT((*core).reg.regs[8], LT)) { _jmp(core); }  }
void _jeq(core_t *core)  { if (CHECKBIT((*core).reg.regs[8], EQ)) { _jmp(core); }  }
void _jgts(core_t *core) { if (CHECKBIT((*core).reg.regs[8], GT)) { _jmps(core); } }
void _jlts(core_t *core) { if (CHECKBIT((*core).reg.regs[8], LT)) { _jmps(core); } }
void _jeqs(core_t *core) { if (CHECKBIT((*core).reg.regs[8], EQ)) { _jmps(core); } }
void _halt() { exit(0);                                }
void _ldp(core_t *core)  { (*core).reg.regs[reg1] = memget((*core).reg.regs[reg2], 16);    }
void _inc(core_t *core)  { (*core).reg.regs[reg1]++; }
void _dec(core_t *core)  { (*core).reg.regs[reg1]--; }
void _jge(core_t *core)  { if (CHECKBIT((*core).reg.regs[8], GT) ||
			       CHECKBIT((*core).reg.regs[8], EQ)) { _jmp(core);  } }
void _jle(core_t *core)  { if (CHECKBIT((*core).reg.regs[8], LT) ||
			       CHECKBIT((*core).reg.regs[8], EQ)) { _jmp(core);  } }
void _jges(core_t *core) { if (CHECKBIT((*core).reg.regs[8], GT) ||
			       CHECKBIT((*core).reg.regs[8], EQ)) { _jmps(core); } }
void _jles(core_t *core) { if (CHECKBIT((*core).reg.regs[8], LT) ||
			       CHECKBIT((*core).reg.regs[8], EQ)) { _jmps(core); } }

void err() {
  printf("FATAL: Invalid opcode '%d'\n", opcode);
  _halt();
}

void eval(core_t *core) {
  switch (opcode) {
  case HALT: _halt(); break;
  CASE(ADD, _add);
  CASE(SUB, _sub);
  CASE(MLT, _mlt);
  CASE(DIV, _div);
  CASE(LDI, _ldi);
  CASE(LDR, _ldr);
  CASE(LDM, _ldm);
  CASE(JMP, _jmp);
  CASE(JMPS, _jmps);
  CASE(DBG, _dbg);
  CASE(AND, _and);
  CASE(OR, _or);
  CASE(XOR, _xor);
  CASE(NOT, _not);
  CASE(RST, _rst);
  CASE(INIT, _nop);
  CASE(ST16, _st16);
  CASE(ST8, _st8);
  CASE(LOOP, _loop);
  CASE(CRR, _crr);
  CASE(CRI, _cri);
  CASE(CRM, _crm);
  CASE(JGT, _jgt);
  CASE(JLT, _jlt);
  CASE(JEQ, _jeq);
  CASE(JGTS, _jgts);
  CASE(JLTS, _jlts);
  CASE(JEQS, _jeqs);
  CASE(JGE, _jge);
  CASE(JLE, _jle);
  CASE(JGES, _jges);
  CASE(JLES, _jles);
  CASE(MOD, _mod);
  CASE(SHL, _shl);
  CASE(SHR, _shr);
  CASE(SHLI, _shli);
  CASE(SHRI, _shri);
  CASE(INT, _int);
  CASE(LDP, _ldp);
  CASE(INC, _inc);
  CASE(DEC, _dec);
  default: err();
  }
}

char* findInit(char* program, size_t sz) {
  int i;
  char* pc = program;
  for (i=0; i<sz; i++) {
    if (program[i] == INIT) { return pc; }
    pc++;
  }
  return NULL;
}

int hwmalloc(size_t sz) {
  sz = sz/2;
  if (hwp >= MAX_HWMEM)      { return -1; }
  if (hwp+sz+1 >= MAX_HWMEM) { return -1; }
  int p = hwp;
  hwp += sz+1;
  return HWMEM_START+p;
}

void interrupt(int16_t i, core_t *core) {
  if (i < 0 || i > MAX_INTERRUPTS || !ivt[i]) {
    logs("Invalid interrupt received: 0x%x", i);
    _halt();
  }
  logs("INT: %x", i);
  if (!ivt[i]()) {
    logs("Interrupt routine failed for: 0x%x", i);
    _halt();
  }
  pthread_mutex_lock(&ivt_mtx);
  (*core).interrupt_lock = TRUE;

  reg_save(core);

  pthread_mutex_unlock(&ivt_mtx);
  // actual handling the interrupt goes here.

  pthread_mutex_lock(&ivt_mtx);
  reg_load(core, -1);
  pthread_mutex_unlock(&ivt_mtx);
}

void regivt(int16_t i, bool (*func)()) {
  if (i < 0 || i > MAX_INTERRUPTS) {
    logs("Tried to register invalid interrupt: %x", i);
    _halt();
  }
  ivt[i] = func;
}

bool load_hardware_object_8(void* dl) {
  hardwareObject8* dev = malloc(sizeof(*dev));
  memset(dev, 0, sizeof(hardwareObject8));
  dev->memory_sz = *(size_t*)dlsym(dl, "memory_sz");
  dev->memory_start = hwmalloc(dev->memory_sz);
  if (!dev->memory_start) {
    logs("hwmalloc failed");
    return FALSE;
  }
  dev->memory_end = dev->memory_start+dev->memory_sz;
  logs("Registering %d bytes of hardware memory @: [0x%x] - [0x%x]", dev->memory_sz, dev->memory_start, dev->memory_end);
  dev->attach = dlsym(dl, "attach");
  dev->detach = dlsym(dl, "detach");
  dev->recv8 = dlsym(dl, "recv8");
  dev->recv16 = dlsym(dl, "recv16");
  dev->sleep = dlsym(dl, "vsleep");
  dev->wake = dlsym(dl, "wake");
  hardwareObject8** devptr = dlsym(dl, "device8");
  *devptr = dev;
  
  if (!dev->attach()) {
    logs("Device attach() failed");
    return FALSE;
  }
  struct hardwareObject ho = {0,8,dev,dl};
  hardware[hardware_cnt] = ho;
  dev->active = TRUE;
  void (*controller)() = dlsym(dl, "controller");
  if (controller) {
    logs("Launching hardware controller");
    pthread_t controller_t;
    pthread_create(&controller_t, NULL, controller, NULL);
  }
  return TRUE;
}

bool load_hardware_object_16(void* dl) {
  return FALSE;
}

bool load_hardware_object(char* path) {
  bool status = FALSE;
  if (hardware_cnt >= MAX_DEV) {
    logs("Device limit reached, cannot attach '%s'", path);
    return status;
  }
  void* dl = dlopen(path, RTLD_NOW);
  if (!dl) {
    logs("dlopen on hardware object '%s' failed: %s", path, dlerror());
    return FALSE;
  }
  bool (*prepare)() = dlsym(dl, "prepare");
  if (prepare && !(prepare())) {
    logs("failed to prepare device '%s'", path);
    return FALSE;
  }
  if (dlsym(dl, "device8"))       { status=load_hardware_object_8(dl);   }
  else if (dlsym(dl, "device16")) { status=load_hardware_object_16(dl);  }
  else { logs("failed to locate device structure in hardware object '%s'", path) }
  if (status) {
    hardware_cnt++;
  }
  return status;
}

bool register_hardware() {
  int i;
  char* paths[MAX_LINE]; memset(paths, 0, MAX_LINE);
  char* list = getenv("DEVICES");
  if (!list) { return TRUE; }
  rstrip(list, MAX_LINE);
  split(list, paths);
  for (i=0; i<MAX_DEV; i++) {
    if (!paths[i]) { break; }
    if (!load_hardware_object(paths[i])) {
      return FALSE;
    }
  }
  return TRUE;
}

void test__(core_t *core) {
  (*core).id++;
}

void run_cpu(core_t * core) {
  core->state=1;
  while (core->state) {
    decode(core);
    dumpInstr(core);
    eval(core);
  }
}

int start_vm(char* argv[]) {
  struct stat progstat;
  int progfd; size_t progsz;

  core_t cpu_core[CORES];

  srand(time(NULL));
  memset(hardware, 0, MAX_DEV*(sizeof(hardwareObject*)));
  memset(ivt, 0, MAX_INTERRUPTS*(sizeof(ivt[0])));
  memset(memory, 0, MEM_BYTES);
  memset(hwmemory, 0, HWMEM_BYTES);
  memset(save, 0, (sizeof(char*)*MAX_JMP));
  instr = malloc(INSTR_BYTES);

  if (!(register_hardware())) {
    logs("Failed to register hardware");
    return 1;
  }

  if (strlen(argv) < 1) {
    printf("Syntax: %s <PROGRAM>\n", argv);
    return 1;
  }

  if ((progfd = open(argv, O_RDONLY)) == -1) {
    printf("Couldn't open program\n");
    return 1;
  }

  if (fstat(progfd, &progstat) == -1) {
    printf("Couldn't stat program");
    return 1;
  }

  progsz = progstat.st_size;
  if (progsz > MAX_PROGSZ) {
    printf("Program size %d exceeds maximum %d\n", progsz, MAX_PROGSZ);
    return 1;
  }
  
  program = malloc(progsz);
  if (!program) {
    printf("Couldn't allocate buffer for program\n");
    return 1;
  }
  memset(program, 0, progsz);

  if (read(progfd, program, MAX_PROGSZ) == -1) {
    printf("Couldn't read program\n");
    return 1;
  }
  
  close(progfd);
  
  if (!(cpu_core[0].reg.pc = findInit(program, progsz))) {
    printf("Couldn't find program init point\n");
    return 1;
  }


  for (char i = 0; i <= CORES; i++) {
  
    cpu_core[i].id = i;
    cpu_core[i].vendor_id = VENDOR;
    cpu_core[i].loop = run_cpu;
  }

  pthread_create(&cpu_core[0].tid, NULL, cpu_core[0].loop, &cpu_core[0]);
  pthread_join(cpu_core[0].tid, NULL);


}

int main(int argc, char* argv[]) {
  start_vm(argv[1]);
  

}
